//
// Created by bourne on 2024/4/17.
//

/***这段代码是 MultiMapManager 类的实现，它属于 fast_planner 命名空间。
 * 这个类负责在多无人机系统中管理多个地图块（MapChunks），用于协作探索和地图构建。***/

#include "plan_env/sdf_map.h"
#include "plan_env/multi_map_manager.h"

#include <visualization_msgs/Marker.h>
#include <fstream>

namespace fast_planner {

MultiMapManager::MultiMapManager() {}

MultiMapManager::~MultiMapManager() {}

void MultiMapManager::setMap(SDFMap *map) {
  this->map_ = map;
}

// 初始化MultiMapManager，设置参数，创建ROS定时器、发布者和订阅者
void MultiMapManager::init() {
  // std::cout << "init multiMapManager" << std::endl;
  node_.param("exploration/drone_id", drone_id_, 1);
  node_.param("exploration/vis_drone_id", vis_drone_id_, -1); // ONLY use for ground node!
  node_.param("exploration/drone_num", map_num_, 2);
  node_.param("multi_map_manager/chunk_size", chunk_size_, 200);

  // 用于定期发送地图块IdxList给其他无人机
  stamp_timer_ = node_.createTimer(ros::Duration(0.1), &MultiMapManager::stampTimerCallback, this);
  // 定时器回调函数，用于定期处理和更新地图块缓冲区中的数据
  chunk_timer_ = node_.createTimer(ros::Duration(0.1), &MultiMapManager::chunkTimerCallback, this);

  // 这里的stamp和chunk的pub和sub话题用send和rec进行区分
  // 实际上所有的飞机的pub和send都用的同一个话题，用drone_id进行区分
  stamp_pub_ = node_.advertise<plan_env::ChunkStamps>("/multi_map_manager/chunk_stamps_send", 10);
  chunk_pub_ = node_.advertise<plan_env::ChunkData>("/multi_map_manager/chunk_data_send", 5000);
  marker_pub_ = node_.advertise<visualization_msgs::Marker>(
      "/multi_map_manager/marker_" + std::to_string(drone_id_), 10);

  stamp_sub_ = node_.subscribe(
      "/multi_map_manager/chunk_stamps_recv", 10, &MultiMapManager::stampMsgCallback, this);
  chunk_sub_ = node_.subscribe("/multi_map_manager/chunk_data_recv", 5000,
           &MultiMapManager::chunkCallback, this, ros::TransportHints().tcpNoDelay());

  // 下面这些数据都是集群每个飞机对应的，因此所有的vector都要resize成map_num_
  multi_map_chunks_.resize(map_num_);
  for (auto &data : multi_map_chunks_) {
    data.idx_list_ = {};
  }

  chunk_boxes_.resize(map_num_);
  for(auto &box : chunk_boxes_) {
    box.valid_ = false;
  }

  chunk_buffer_.resize(map_num_);
  buffer_map_.resize(map_num_);

  last_chunk_stamp_time_.resize(map_num_);
  for(auto &time : last_chunk_stamp_time_) {
    time = 0.0;
  }
  // std::cout << "multiMapManager init done" << std::endl;

  /***
  std::cout << "test the idx list operation" << std::endl;
  // find intersect
  std::cout << "====find intersect=====" << std::endl;
  vector<int> test1 = {1, 20};
  vector<int> test2 = {8, 30};
  vector<int> test3 = {20, 30};
  vector<int> test4 = {1000, 10001};
  int minr, maxr;
  std::cout << findIntersect(test1[0], test1[1], test2[0], test2[1], minr, maxr) << std::endl;
  std::cout << "test1 and test2 intersect" << minr << ", " << maxr << std::endl;
  std::cout << findIntersect(test3[0], test3[1], test2[0], test2[1], minr, maxr) << std::endl;
  std::cout << "test3 and test2 intersect" << minr << ", " << maxr << std::endl;
  std::cout << findIntersect(test1[0], test1[1], test4[0], test4[1], minr, maxr) << std::endl;
  std::cout << "test1 and test4 intersect" << minr << ", " << maxr << std::endl;

  // find missed
  std::cout << "=====find missed=====" << std::endl;
  vector<int> self0 = {1, 1000};
  vector<int> self1 = {5, 100, 105, 105, 120, 300, 400, 600, 700, 1000};
  vector<vector<int>> selfs = {self0, self1};

  vector<int> other0 = {};
  vector<int> other1 = {10, 80};
  vector<int> other2 = {10, 80, 150, 200, 501, 550};
  vector<vector<int>> others = {other0, other1, other2};

  for(int i = 0; i < selfs.size(); i ++) {
    for(int j = 0; j < others.size(); j ++) {
      std::cout << "self " << i << " other "  << j << std::endl;
      vector<int> missed;
      findMissedChunkIds(selfs[i], others[j], missed);
      for (int k = 0; k < missed.size(); k ++) {
        std::cout << missed[k] << " ";
      }
      std::cout << std::endl;
    }
  }

  // merge chunk ids
  std::cout << "=====merge chunk ids=====" << std::endl;
  vector<int> input0 = {1, 1000};
  vector<int> input1 = {1, 9, 81, 1000};
  vector<int> input2 = {1, 7, 85, 140, 250, 450, 560, 900};
  vector<vector<int>> inputs = { input0, input1, input2};
  for (int i = 0; i < others.size(); i++) {
    for (int j = 0; j < inputs.size(); j++) {
      std::cout << "other " << i << " input " << j << std::endl;
      vector<int> output;
      mergeChunkIds(others[i], inputs[j], output);
      std::cout << "===Merged result: ";
      for (auto id : output) std::cout << id << ", ";
      std::cout << "" << std::endl;
    }
  }
  std::cout << "multiMap manager init done" << std::endl;
  ***/
}






// updateMapChunk函数用于处理新的地图地址数据，只用于处理本无人机数据
// 当一帧depth和pose来了以后，进入到inputPointCloud中后
// 会对应更新地图中对应voxel的占据情况，然后将其address数据传入到mm中
void MultiMapManager::updateMapChunk(const vector<uint32_t> &adrs) {
//  std::cout << adrs.size() << std::endl;
  // 将新的地图地址添加到adr_buffer_缓冲区中
  adr_buffer_.insert(adr_buffer_.end(), adrs.begin(), adrs.end());
  // 如果缓冲区的大小超过了预设的chunk_size_，则需要创建新的地图块
  if (adr_buffer_.size() >= chunk_size_) {
    // 从缓冲区中按chunk_size_分割出地图块
    int i = 0;
    for ( ; i + chunk_size_ < adr_buffer_.size(); i += chunk_size_) {
      MapChunk chunk; // 创建一个新的MapChunk对象
      // 将缓冲区中的一部分地址数据复制到chunk的voxel_adrs_中
      chunk.voxel_adrs_.insert(
          chunk.voxel_adrs_.end(), adr_buffer_.begin() + i, adr_buffer_.begin() + i + chunk_size_);
      // 设置chunk的索引，这里idx_是从1开始的，表示第几个chunk
      chunk.idx_ = multi_map_chunks_[drone_id_ - 1].chunks_.size() + 1;
      // 设置chunk的状态，need_query_为true表示需要查询
      // 在体素刚刚进入到multi_map_chunks_的时候，只有adr消息，还没有提取occ消息，所以是need_query
      //  empty_为false表示不为空
      chunk.need_query_ = true;
      chunk.empty_ = false;
      // 将新的chunk添加到对应无人机的chunks列表中
      multi_map_chunks_[drone_id_ - 1].chunks_.push_back(chunk);
    }
    // 如果idx_list_为空，则初始化为{1, 1}
    // 因为这个函数处理的是自己建图的进来的voxel，如果是空的就要先初始化为1，1
    // 第一帧数据进来以后，肯定是从这个个idx肯定是1开始的，后面直接继续增长就ok了
    // 这个idx指的是chunk的idx，是1，2，3，4，
    // 在这里用区间表示，一开始是1，1，后面第二个来了以后变成1，2
    if (multi_map_chunks_[drone_id_ - 1].idx_list_.empty()) {
      multi_map_chunks_[drone_id_ - 1].idx_list_ = { 1, 1 };
    }
    // 更新idx_list_的最后一个元素为最新添加的chunk的idx
    multi_map_chunks_[drone_id_ - 1].idx_list_.back() = multi_map_chunks_[drone_id_ - 1].chunks_.back().idx_;

    // 移除已经插入到multi_map_chunks_列表中的地址数据
    vector<uint32_t> tmp;
    tmp.insert(tmp.end(), adr_buffer_.begin() + i, adr_buffer_.end());
    adr_buffer_ = tmp;
  }
//  std::cout << "idx size" << multi_map_chunks_[drone_id_ - 1].idx_list_.size() << std::endl;
//  for (int i = 0; i < multi_map_chunks_[drone_id_ - 1].idx_list_.size(); i++) {
//    std::cout<< multi_map_chunks_[drone_id_ - 1].idx_list_[i] << " ";
//  }
//  std::cout << std::endl;
}




// 函数通过遍历 chunk_boxes_ 向量，收集所有有效地图块的最小和最大边界点。
void MultiMapManager::getChunkBoxes(
    vector<Eigen::Vector3d>& mins,  // 存储每个地图块的最小边界点的向量
    vector<Eigen::Vector3d>& maxs,  // 存储每个地图块的最大边界点的向量
    bool reset)  // 指示是否重置地图块边界框的有效状态
{
  // 遍历chunk_boxes_中的每个地图块边界框
  for (auto& box : chunk_boxes_) {
    // 如果当前地图块边界框有效
    if (box.valid_) {
      // 将最小边界点添加到mins向量中
      mins.push_back(box.min_);
      // 将最大边界点添加到maxs向量中
      maxs.push_back(box.max_);
      // 如果reset为true，则重置当前地图块边界框的有效状态为false
      if (reset) box.valid_ = false;
    }
  }
}





// 因为ChunkData是一个个发送的，所以如果对方中间丢掉了一部分的话
// multi_map_chunks_的chunksdata那个list就会出现断断续续的情况
void MultiMapManager::sendChunks(
    const int &chunk_drone_id, const int &to_drone_id, const vector<int> &idx_list) {
  // 获取当前无人机的地图块数据
  auto& data = multi_map_chunks_[chunk_drone_id - 1];

  // 遍历idx_list中的每个地图块索引对
  for (int i = 0; i < idx_list.size(); i += 2) {
    for (int j = idx_list[i]; j <= idx_list[i + 1]; ++j) {
      plan_env::ChunkData msg;
      msg.from_drone_id = drone_id_;
      msg.to_drone_id = to_drone_id;
      msg.chunk_drone_id = chunk_drone_id;
      msg.idx = data.chunks_[j - 1].idx_;
      msg.voxel_adrs = data.chunks_[j - 1].voxel_adrs_;
      // 如果是本无人机自己生成的chunks，就将其need query置false，表明其已经查询过了
      // need_query证明这个chunk刚刚建图出来，只有adr信息，没有occ消息，需要在发送之前进行查询
      if (chunk_drone_id == drone_id_ && data.chunks_[j - 1].need_query_) {
        // Should query the occ info in map if they are still empty
        getOccOfChunk(data.chunks_[j - 1].voxel_adrs_, data.chunks_[j - 1].voxel_occ_);
        data.chunks_[j - 1].need_query_ = false;
      }
      msg.voxel_occ = data.chunks_[j - 1].voxel_occ_;

      // // Swarm communication
      // msg.pos_x = drone_pos_[0];
      // msg.pos_y = drone_pos_[1];
      // msg.pos_z = drone_pos_[2];

      chunk_pub_.publish(msg);
      // std::cout << "Drone " << drone_id_ << " send chunk " << msg.idx << " of drone "
      //           << int(msg.chunk_drone_id) << " to drone " << int(msg.to_drone_id) << std::endl;
    }
  }
}





void MultiMapManager::getOccOfChunk(const vector<uint32_t>& adrs, vector<uint8_t>& occs) {
  for (auto adr : adrs) {
    uint8_t occ = map_->md_->occupancy_buffer_[adr] > map_->mp_->min_occupancy_log_ ? 1 : 0;
    occs.push_back(occ);
  }
}





// 将地图块插入到地图中
void MultiMapManager::insertChunkToMap(
    const MapChunk &chunk, const int &drone_id) {
  // // Transform from other drone's local frame to this drone's
  // Eigen::Vector4d transform;
  // map_->getBaseCoor(drone_id, transform);

  // double yaw = transform[3];
  // Eigen::Matrix3d rot;
  // rot << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;
  // Eigen::Vector3d trans = transform.head<3>();

  // 遍历地图块中的每个体素地址
  for (int i = 0; i < chunk.voxel_adrs_.size(); ++i) {
    // Insert occ info
    // 获取当前体素的地址
    auto& adr = chunk.voxel_adrs_[i];

    // 将地址转换为地图索引
    Eigen::Vector3i idx;
    adrToIndex(adr, idx);

    // 将索引转换为地图中的位置
    Eigen::Vector3d pos;
    map_->indexToPos(idx, pos);

    // 如果体素位置不在地图范围内，则跳过
    // pos = rot * pos + trans;
    if (!map_->isInMap(pos)) continue;

    // 将位置转换回索引，并获取对应的地址
    map_->posToIndex(pos, idx);
    auto adr_tf = map_->toAddress(idx);

    // map_->md_->occupancy_buffer_[adr] =
    //     chunk.voxel_occ_[i] == 1 ? map_->mp_->clamp_max_log_ : map_->mp_->clamp_min_log_;
    // 更新地图的占据信息
    // 根据体素的占据状态（chunk.voxel_occ_[i]），更新地图的占据缓冲区
    map_->md_->occupancy_buffer_[adr_tf] =
        chunk.voxel_occ_[i] == 1 ? map_->mp_->clamp_max_log_ : map_->mp_->clamp_min_log_;

    // Update the chunk box
    // 更新无人机的地图块边界框
    if (chunk_boxes_[drone_id - 1].valid_) {
      // 如果边界框已经有效，则更新边界框的最小和最大值
      for (int k = 0; k < 3; ++k) {
        chunk_boxes_[drone_id - 1].min_[k] = min(chunk_boxes_[drone_id - 1].min_[k], pos[k]);
        chunk_boxes_[drone_id - 1].max_[k] = max(chunk_boxes_[drone_id - 1].max_[k], pos[k]);
      }
    } else {
      // 如果边界框尚未有效，则初始化边界框
      chunk_boxes_[drone_id - 1].min_ = chunk_boxes_[drone_id - 1].max_ = pos;
      chunk_boxes_[drone_id - 1].valid_ = true;
    }

    // Update the all box
    // 更新整个地图的边界框
    for (int k = 0; k < 3; ++k) {
      map_->md_->all_min_[k] = min(map_->md_->all_min_[k], pos[k]);
      map_->md_->all_max_[k] = max(map_->md_->all_max_[k], pos[k]);
    }
    // Inflate for the occupied
    // 如果体素被占据，对占据的体素进行膨胀处理
    if (chunk.voxel_occ_[i] == 1) {
      static const int inf_step = ceil(map_->mp_->obstacles_inflation_ / map_->mp_->resolution_);
      for (int inf_x = -inf_step; inf_x <= inf_step; ++inf_x)
        for (int inf_y = -inf_step; inf_y <= inf_step; ++inf_y)
          for (int inf_z = -inf_step; inf_z <= inf_step; ++inf_z) {
            Eigen::Vector3i inf_pt(idx[0] + inf_x, idx[1] + inf_y, idx[2] + inf_z);
            if (!map_->isInMap(inf_pt)) continue;
            int inf_adr = map_->toAddress(inf_pt);
            map_->md_->occupancy_buffer_inflate_[inf_adr] = 1;
          }
    }
  }

}





// 将地图中的地址转换为三维索引
void MultiMapManager::adrToIndex(const uint32_t& adr, Eigen::Vector3i& idx) {
  // 地图地址值
  uint32_t tmp_adr = adr;
  // 计算地图在x和y方向上的体素数量，用于计算索引
  const int a = map_->mp_->map_voxel_num_[1] * map_->mp_->map_voxel_num_[2];
  const int b = map_->mp_->map_voxel_num_[2];

  // 计算索引的x分量
  idx[0] = tmp_adr / a;
  // 更新临时地址值，移除已计算的x分量
  tmp_adr = tmp_adr % a;
  // 计算索引的y分量
  idx[1] = tmp_adr / b;
  // 更新临时地址值，移除已计算的y分量
  idx[2] = tmp_adr % b;
}





// 定时器回调函数，用于定期发送地图块IdxList给其他无人机
// 说明自己有哪些地图块
// 当其他飞机接收到以后，会检测其他飞机有而这个飞机没有的地图块，然后把这些地图块发给他
void MultiMapManager::stampTimerCallback(const ros::TimerEvent &e) {
  // 创建一个ChunkStamps消息，用于存储时间戳和相关无人机的ID
  plan_env::ChunkStamps msg;
  msg.from_drone_id = drone_id_; // 设置发送者无人机的ID
  msg.time = ros::Time::now().toSec(); // 获取当前时间的时间戳

  // 遍历multi_map_chunks_中的每个无人机的chunks数据
  for (auto& chunks : multi_map_chunks_) {
    plan_env::IdxList idx_list; // 对于每个无人机存储在本地的数据，创建一个IdxList
    idx_list.ids = chunks.idx_list_;
    msg.idx_lists.push_back(idx_list); // 将IdxList添加到ChunkStamps消息中
  }
  // 发送ChunkStamps消息
  stamp_pub_.publish(msg);
  return;

  // Test, visualize the chunks
  // 只可视化本无人机自己生成的chunks，而且只可视化occ的部分
  static int pub_num = 0;
  static int pub_id = 0;

  visualization_msgs::Marker mk;
  mk.header.frame_id = "world";
  mk.ns = "own";
  mk.header.stamp = ros::Time::now();
  mk.type = visualization_msgs::Marker::CUBE_LIST;
  mk.pose.orientation.w = 1.0;
  mk.color.r = 1;
  mk.color.g = 0;
  mk.color.b = 0;
  mk.color.a = 0.5;
  mk.scale.x = 0.1;
  mk.scale.y = 0.1;
  mk.scale.z = 0.1;

  auto& data = multi_map_chunks_[drone_id_ - 1];
  if (pub_num < data.chunks_.size()) {
    for (int i = pub_num; i < data.chunks_.size(); ++i) {
      // std::cout << "Chunk size : " << chunk.voxel_adrs_.size() << std::endl;
      auto& chunk = data.chunks_[i];
      // std::cout << chunk.stamp_ - first_stamp << ", ";

      for (auto adr : chunk.voxel_adrs_) {
        Eigen::Vector3i idx;
        Eigen::Vector3d pos;
        adrToIndex(adr, idx);
        map_->indexToPos(idx, pos);
        if (map_->getOccupancy(idx) == SDFMap::OCCUPIED) {
          geometry_msgs::Point pt;
          pt.x = pos[0];
          pt.y = pos[1];
          pt.z = pos[2];
          mk.points.push_back(pt);
        }
      }
    }
    if (!mk.points.empty()) {
      pub_num = data.chunks_.size();
      mk.id = pub_id++;
      marker_pub_.publish(mk);
    }
  }
}



// 定时器回调函数，用于定期处理和更新地图块缓冲区中的数据
// 函数通过遍历每个无人机的地图块缓冲区，对缓冲区中的地图块进行排序，并计算索引列表。
// 然后，它更新 ChunksData 结构体中的地图块列表和索引列表。
void MultiMapManager::chunkTimerCallback(const ros::TimerEvent &e) {
  // Not process chunk until swarm basecoor transform is available
  // Eigen::Vector4d tmp;
  // if (!map_->getBaseCoor(1, tmp)) {
  //   ROS_WARN("basecoor not available yet.");
  //   return;
  // }

  // auto t1 = ros::Time::now();

  // 遍历每个无人机的地图块缓冲区
  for (int i = 0; i < chunk_buffer_.size(); i ++) {
    auto& buffer = chunk_buffer_[i]; // 当前无人机的地图块缓冲区
    if (buffer.empty()) continue; // 如果缓冲区为空，跳过当前迭代

    // Compute the idx list of buffered chunks
    // 对缓冲区中的地图块按索引排序
    sort(buffer.begin(), buffer.end(),
         [](const plan_env::ChunkData& chunk1, const plan_env::ChunkData& chunk2) {
           return chunk1.idx < chunk2.idx;
         });
    // 计算缓冲区中地图块的索引列表
    vector<int> idx_list = { int(buffer.front().idx) };
    int last_idx = idx_list[0];
    for (int j = 1; j < buffer.size(); ++j) {
      if (buffer[j].idx - last_idx > 1) {
        idx_list.push_back(last_idx);
        idx_list.push_back(buffer[j].idx);
      }
      last_idx = buffer[j].idx;
    }
    idx_list.push_back(last_idx);


    // std::cout << "process drone " << i + 1 << "'s chunks, input idx list: ";
    // for (auto id : idx_list) std::cout << id << ", ";
    // std::cout << "" << std::endl;

    // Update ChunksData's chunks_
    // 更新ChunksData的chunks_列表
    auto& chunks_data = multi_map_chunks_[i];

    // std::cout << "self idx list " << i + 1 << ": ";
    // for (auto id : chunks_data.idx_list_) std::cout << id << ", ";
    // std::cout << "" << std::endl;

    // Add placeholder for chunks
    // 为新的地图块添加占位符
    int len_inc = last_idx - chunks_data.chunks_.size();
    for (int j = 0; j < len_inc; ++j) {
      chunks_data.chunks_.push_back(MapChunk());
      auto& back_chunk = chunks_data.chunks_.back();
      back_chunk.idx_ = chunks_data.chunks_.size();
      back_chunk.empty_ = true;
      back_chunk.need_query_ = false;
    }

    // Process data in buffer
    // 处理缓冲区中的每个地图块数据
    for (auto msg : buffer) {
      auto& chunk = chunks_data.chunks_[msg.idx - 1];
      if (chunk.empty_) {  // Only insert a chunk once
        chunk.voxel_adrs_ = msg.voxel_adrs;
        chunk.voxel_occ_ = msg.voxel_occ;
        insertChunkToMap(chunk, msg.chunk_drone_id);
        chunk.empty_ = false;
      }
    }

    // Update ChunksData's idx_list_
    // 更新ChunksData的idx_list_
    vector<int> union_list;
    mergeChunkIds(idx_list, chunks_data.idx_list_, union_list);
    chunks_data.idx_list_ = union_list;

    // std::cout << "merged idx list " << i + 1 << ": ";
    // for (auto id : union_list) std::cout << id << ", ";
    // std::cout << "" << std::endl;

    buffer.clear();
    buffer_map_[i].clear();
  }
}





// 这个函数用来调用其他飞机发送的stamp消息
// 调用得到了其他飞机的stamp消息以后，和自己储存的stamp消息进行对比，看对方缺什么
// 然后将缺的部分的地图的占据和空闲的状态发送过去，调用的是send函数
void MultiMapManager::stampMsgCallback(const plan_env::ChunkStampsConstPtr &msg) {
  // 如果消息是从当前无人机发送的，或者当前无人机是地面节点（不发送地图块），则直接返回
  if (msg->from_drone_id == drone_id_) return;
  // 如果droneid和mapnum相等，这就是最后一个drone就是地面站节点
  // 地面站节点不发送stamp，因为它只接收，但是不建图
  if (drone_id_ == map_num_) return;

  // 检查消息的时间戳，如果距离上次接收到同一无人机的时间戳太近，则忽略该消息
  // 这可以防止在短时间内接收到大量重复的消息
  if (msg->time - last_chunk_stamp_time_[msg->from_drone_id - 1] < 0.3) return;
  last_chunk_stamp_time_[msg->from_drone_id - 1] = msg->time; // 更新最后接收到该无人机时间戳的时间

  // 遍历multi_map_chunks_中的每个无人机的chunks数据
  for (int i = 0; i < multi_map_chunks_.size(); i ++) {
    if (i == msg->from_drone_id - 1) continue; // 如果当前迭代的是消息来源的无人机，跳过
    // 找出当前无人机缺少的地图块索引
    // 找出发送这个stamp的消息的无人机缺少的地图索引块，将其发给对应的无人机/地面站节点
    vector<int> missed;
    findMissedChunkIds(multi_map_chunks_[i].idx_list_, msg->idx_lists[i].ids, missed);
    sendChunks(i + 1, msg->from_drone_id, missed);
  }
}





// 接收其他无人机发送过来的，本无人机缺失的chunkdata
// 然后push进入到一个buffer中等待处理
void MultiMapManager::chunkCallback(const plan_env::ChunkDataConstPtr &msg) {
  // Receive chunks from other drones, store them in chunk buffer
  if (msg->from_drone_id == drone_id_) return;
  // if(msg->to_drone_id != drone_id_) return;

  // Ignore chunks that are in the insertion buffer
  // 如果当前地图块已经在插入缓冲区中，则忽略它
  if (buffer_map_[msg->chunk_drone_id - 1].find(msg->idx) !=
      buffer_map_[msg->chunk_drone_id - 1].end())
    return;

  // 将接收到的地图块数据添加到对应无人机的缓冲区中
  chunk_buffer_[msg->chunk_drone_id - 1].push_back(*msg);
  // 在缓冲区的哈希映射中标记当前地图块索引已接收
  buffer_map_[msg->chunk_drone_id - 1][msg->idx] = 1;
}





// 辅助函数，用来检测other_idx_list相比self_idx_list，缺少的东西
// const vector<int>& self_idx_list 当前无人机拥有的地图块索引列表
// const vector<int>& other_idx_list 其他无人机拥有的地图块索引列表
// vector<int>& miss_ids 缺少的地图块索引列表

// idx的数据都是一对一对的，分别是一维数组里面起始的idx和最终的idx
void MultiMapManager::findMissedChunkIds(
    const vector<int> &self_idx_list, const vector<int> &other_idx_list,
    vector<int> &miss_ids) {
  // 如果other_idx_list为空，说明没有其他无人机的索引信息，其他无人机缺少所有地图块
  if (other_idx_list.empty()) {
    miss_ids = self_idx_list;
    return;
  }

  // 创建一个列表not_in_other，用于存储other_idx_list中不存在的索引范围
  vector<int> not_in_other;
  // 如果other_idx_list的第一个索引大于1，说明存在一个从"1"到"other_idx_list第一个索引-1"的缺失范围
  if (other_idx_list[0] > 1) {
    not_in_other.push_back(1);
    not_in_other.push_back(other_idx_list[0]-1);
  }
  // 遍历other_idx_list中的每个索引对, 计算每个索引对之间的缺失范围，并添加到not_in_other中
  for (int i = 1; i < other_idx_list.size(); i += 2) {
    not_in_other.push_back(other_idx_list[i] + 1);
    if (i == other_idx_list.size() - 1) {
      // 如果是最后一个索引对，添加一个无穷大的值，表示没有上限的缺失范围
      int infinite = std::numeric_limits<int>::max();
      not_in_other.push_back(infinite);
    } else {
      not_in_other.push_back(other_idx_list[i + 1] - 1);
    }
  }

  // Compute the intersection of self and not_in_other (brute-force, O(n^2))
  // 计算self_idx_list和not_in_other的交集，即self有other没有的地图块索引
  for (int i = 0; i < self_idx_list.size(); i += 2) {
    for (int j = 0; j < not_in_other.size(); j += 2) {
      int minr, maxr;
      // 调用findIntersect函数来找出两个索引范围的交集
      if (findIntersect(self_idx_list[i], self_idx_list[i+1], not_in_other[j], not_in_other[j+1],
                        minr, maxr)) {
        // 如果存在交集，将交集的最小和最大索引添加到缺少的索引列表中
        miss_ids.push_back(minr);
        miss_ids.push_back(maxr);
      }
    }
  }
}





// 辅助函数，用于判断两个整数范围是否有交集，并计算交集的边界
bool MultiMapManager::findIntersect(
    const int &min1, const int &max1, // 第一个范围的最小值和最大值
    const int &min2, const int &max2, // 第二个范围的最小值和最大值
    int &minr, int &maxr) // 交集的最小值和最大值的引用
{
  minr = max(min1, min2); // 计算交集的最小值，取两个最小值中的较大者
  maxr = min(max1, max2); // 计算交集的最大值，取两个最大值中的较小者

  // 判断交集是否有效，即交集的最小值是否小于等于最大值
  if (minr <= maxr) return true; // 如果有交集，返回true
  return false; // 如果没有交集，返回false
}





// 合并两个地图块索引列表，返回一个新的合并后的列表
void MultiMapManager::mergeChunkIds(
    const vector<int> &list1, const vector<int> &list2, vector<int> &output) {
  // 如果第一个列表为空，直接将第二个列表的内容复制到输出列表
  if (list1.empty()) {
    output = list2;
    return;
  }

  output = list1;
  int tmp1, tmp2;
  // For each interval in list2, merge it into output list
  // 遍历第二个列表中的每个索引区间, 将其合并到输出列表中
  // 具体操作：对list2中的元素进行遍历，对比output中的各个元素
  // 如果有交集，就将这两个合并，如果没有交集，就将list2中的这个元素插入到output的合适位置
  for (int i = 0; i < list2.size(); i += 2) {
    bool intersect = false;

    // 在输出列表中查找与当前区间的交集
    // 如果有，就直接对当前的output的对应区间进行更改就可以
    for (int j = 0; j < output.size(); j += 2) {
      if (findIntersect(output[j], output[j+1], list2[i], list2[i+1], tmp1, tmp2)) {
        output[j] = min(output[j], list2[i]);
        output[j + 1] = max(output[j + 1], list2[j + 1]);
        intersect = true;
      }
    }

    // 如果没有找到交集，将当前区间插入到输出列表的适当位置
    if (!intersect) {  // Insert the interval in appropriate position
      vector<int> tmp = { list2[i], list2[i + 1] };
      if (list2[i + 1] < output.front()) {
        // 如果当前区间的结束索引小于输出列表的第一个区间的起始索引，插入到开头
        output.insert(output.begin(), tmp.begin(), tmp.end());
      } else if (list2[i] > output.back()) {
        // 如果当前区间的起始索引大于输出列表的最后一个区间的结束索引，插入到末尾
        output.insert(output.end(), tmp.begin(), tmp.end());
      } else {
        // 否则，找到当前区间应该插入的位置并插入
        for (auto iter = output.begin() + 1; iter != output.end(); iter += 2) {
          if (*iter < list2[i] && *(iter + 1) > list2[i + 1]) {
            output.insert(iter + 1, tmp.begin(), tmp.end());
            break;
          }
        }
      }
    }
    /***
    std::cout << "output before remove: ";
    for (auto id : output) std::cout << id << ", ";
    std::cout << "" << std::endl;
     ***/
    // Remove redundant idx
    // 移除输出列表中连续重复的索引
    for (auto iter = output.begin() + 1; iter != output.end() - 1;) {
      if (*iter >= *(iter + 1) - 1) {
        iter = output.erase(iter);
        iter = output.erase(iter);
      } else {
        iter += 2;
      }
    }
    /***
    std::cout << "output after remove: ";
    for (auto id : output) std::cout << id << ", ";
    std::cout << "" << std::endl;
     ***/
  }
}

} // namespace fast_planner